# -*- coding: utf-8 -*-
"""
    @project: pythonProject
    @Author：HanYonghua
    @file： farrow2.py
    @date：2025/7/23 8:25
    @blogs: https://www.ncatest.com.cn
"""

import numpy as np
import matplotlib.pyplot as plt

#A0 = (((c00*uk+c01)*uk+c02)*uk+c03)
#A1 = (((c10*uk+c11)*uk+c12)*uk+c13)
#A2 = (((c20*uk+c21)*uk+c22)*uk+c23)
#A3 = (((c30*uk+c31)*uk+c32)*uk+c33)

# y[n] = A0*x[0]+A1*x[1]+A2*x[2]+A3*x[3]

# Farrow插值函数（保持不变）
def farrow_lagrange_interpolation(x, I=3, D=2):
    #3阶系数
    v0 = np.array([-1 / 6, 1 / 2, -1 / 2, 1 / 6])
    #2阶系数
    v1 = np.array([1 / 2, -1, 1 / 2, 0])
    #1阶系数
    v2 = np.array([-1 / 3, -1 / 2, 1, -1 / 6])
    #0阶系数(常数项)
    v3 = np.array([0, 1, 0, 0])

    step_factor = D / I
    lengthx = len(x)
    xbuf = np.zeros(4)
    y = np.zeros(round(lengthx * I / D) + 4)
    k = 0
    pha = 0
    x = np.append(x, [0, 0])

    for i in range(len(x)):
        xbuf[3] = xbuf[2]
        xbuf[2] = xbuf[1]
        xbuf[1] = xbuf[0]
        xbuf[0] = x[i]

        c0 = np.dot(xbuf, v0)
        c1 = np.dot(xbuf, v1)
        c2 = np.dot(xbuf, v2)
        c3 = np.dot(xbuf, v3)

        if i == 1:
            y[k] = x[0]
            k += 1

        if i > 1:
            pha += 1
            while pha >= step_factor:
                pha -= step_factor
                uk = pha
                yy4 = c0 * uk
                yy3 = (yy4 + c1) * uk
                yy2 = (yy3 + c2) * uk + c3
                y[k] = yy2
                k += 1
    return y[:k]


# 生成测试信号
fs = 2.5e3
fc = 2e2
t = np.arange(0, 200 / fc, 1 / fs)
x = np.cos(2 * np.pi * fc * t)

# 执行插值
I, D = 3, 2
y = farrow_lagrange_interpolation(x, I, D)


# 正确的频谱计算函数
def plot_spectrum(signal, fs, title, ax):
    N = len(signal)
    window = np.hamming(N)
    spectrum = np.fft.fft(signal * window)
    magnitude = np.abs(spectrum)[:N // 2] * 2 / N  # 正确归一化
    freq = np.fft.fftfreq(N, 1 / fs)[:N // 2]
    ax.plot(freq, 20 * np.log10(magnitude + 1e-10))  # 避免log(0)
    ax.set_title(title)
    ax.set_xlabel('Frequency (Hz)')
    ax.set_ylabel('Magnitude (dB)')
    ax.grid(True)
    ax.set_xlim(0, fs / 2)


# 绘制时域和频域结果
plt.figure(figsize=(12, 8))

# 时域对比
plt.subplot(2, 1, 1)
plt.plot(t[:130], x[:130], '--*', label=f'Original (fs={fs} Hz)')
t_y = np.arange(len(y)) * D / (I * fs)  # 修正时间轴
plt.plot(t_y[:130], y[:130], '--o', label=f'Resampled (fs={fs * I / D:.1f} Hz)')
plt.legend()
plt.title('Time Domain Comparison')
plt.xlabel('Time (s)')
plt.ylabel('Amplitude')
plt.grid(True)

# 频域对比
plt.subplot(2, 1, 2)
plot_spectrum(x, fs, 'Input Spectrum', plt.gca())
plot_spectrum(y, fs * I / D, 'Output Spectrum', plt.gca())
plt.legend(['Input', 'Output'])
plt.tight_layout()
plt.show()